#include "node_ll_4d4s.h"
#include "common_tool/angle_helper.h"
#include "perpend_point2d_helper.h"
#include <cmath>
#include <limits>
#include <climits>
#include "common_tool/geometry.hpp"
#include "common_tool/csv_writer.h"
#include <memory>
#include <chrono>
#include "node_config_path.h"
#include "eliminate_range.hpp"

namespace auto_ros
{
namespace control
{
NodeLL4d4s::NodeLL4d4s(
	double loop_rate, std::string node_yaml_file_name)
	: NodeBase(loop_rate, node_yaml_file_name),
	  speed_controller_(0.2, 0.001, 0, 1 / loop_rate, -0.2, 0.2, -0.2, 0.2)

{
	// filter parameters

	const float samplingrate = loop_rate; // Hz
	const float cutoff_frequency = 10.0;  // Hz
	// calc the coefficients
	low_pass_f_.setup(samplingrate, cutoff_frequency);
	std::string vehicle_lmpc_yaml_file = "vehicle_lmpc.yaml";
	VehicleLMpcFactory lmpc_factory;
	mpc_ptr_ = lmpc_factory.CreateSharedObject("VehicleLL4d4sMpc",
											   node_config_path + "VehicleLL4d4sMpc/", vehicle_lmpc_yaml_file, 50);
	if (mpc_ptr_ == nullptr)
	{
		std::cout << "\033[31m Error:NodeLL4d4s::NodeLL4d4s failed,can not create mpc_ptr_ \033[0m" << std::endl;
		throw 100;
	}

	unsigned int state_dim = mpc_ptr_->vehicle_dyn_ptr_->state_dim();
	unsigned int control_dim = mpc_ptr_->vehicle_dyn_ptr_->control_dim();
	unsigned int distur_dim = mpc_ptr_->vehicle_dyn_ptr_->distur_dim();
	//init mpc_ptr_ update_imput	Eigen::MatrixXd q_;
	x0_ = Eigen::VectorXd::Zero(state_dim);
	u_fb_ = Eigen::VectorXd::Zero(control_dim);
	//std::cout << mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_ << std::endl;
}

void NodeLL4d4s::HardBrake()
{
	control_cmd_.brake = 0;
	control_cmd_.throttle = 0;
	speed_controller_.resetIntergration();
}
void NodeLL4d4s::HardBrake(double brake_pre)
{
	control_cmd_.brake = brake_pre;
	control_cmd_.throttle = 0;
	speed_controller_.resetIntergration();
}
bool NodeLL4d4s::UpdateInput()
{
	double temp_angle = auto_ros::common_tool::angle_mins_pi2pi(ref_point_interp_.theta) + 0.5 * M_PI;
	double heading_error = auto_ros::common_tool::angle_mins_pi2pi(localizition_msg_.heading - ref_point_interp_.theta);
	double planning_speed;
	if (node_yaml_["is_overwrite_speed"].as<int>() == 1)
	{
		planning_speed = node_yaml_["test_speed"].as<double>();
	}
	else
	{
		planning_speed = ref_point_interp_.v;
	}

	x0_ << localizition_msg_.vx - planning_speed,
		localizition_msg_.vy,
		localizition_msg_.avz / 180 * M_PI,
		-lateral_error_, heading_error; //localizition_msg_.ve * std::cos(temp_angle) + localizition_msg_.vn * std::sin(temp_angle); //localizition_msg_.ve * std::cos(temp_angle) + localizition_msg_.vn * std::sin(temp_angle);

	double max_f_angle_deg = mpc_ptr_->mpc_yaml_["max_tire_f_angle"].as<double>();
	double max_r_angle_deg = mpc_ptr_->mpc_yaml_["max_tire_r_angle"].as<double>();
	double tire_f_angle = (clamp(chassis_msg_.steering_angle_f,
								 -max_f_angle_deg,
								 max_f_angle_deg)) /
						  180 * M_PI;
	double tire_r_angle = (clamp(chassis_msg_.steering_angle_r,
								 -max_r_angle_deg,
								 max_r_angle_deg)) /
						  180 * M_PI;
	//calc acc
	double acc_fl = (-chassis_msg_.brake_pre_fl * mpc_ptr_->mpc_yaml_["brake_nm_per_Mpa"].as<std::vector<double>>()[0] +
					 chassis_msg_.torque_fl) /
					mpc_ptr_->mpc_yaml_["tire_raduis"].as<double>() /
					mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["mass"].as<double>();
	double acc_fr = (-chassis_msg_.brake_pre_fr * mpc_ptr_->mpc_yaml_["brake_nm_per_Mpa"].as<std::vector<double>>()[1] +
					 chassis_msg_.torque_fr) /
					mpc_ptr_->mpc_yaml_["tire_raduis"].as<double>() /
					mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["mass"].as<double>();
	double acc_rl = (-chassis_msg_.brake_pre_rl * mpc_ptr_->mpc_yaml_["brake_nm_per_Mpa"].as<std::vector<double>>()[2] +
					 chassis_msg_.torque_rl) /
					mpc_ptr_->mpc_yaml_["tire_raduis"].as<double>() /
					mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["mass"].as<double>();
	double acc_rr = (-chassis_msg_.brake_pre_rr * mpc_ptr_->mpc_yaml_["brake_nm_per_Mpa"].as<std::vector<double>>()[3] +
					 chassis_msg_.torque_rr) /
					mpc_ptr_->mpc_yaml_["tire_raduis"].as<double>() /
					mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["mass"].as<double>();
	u_fb_ << tire_f_angle, tire_r_angle, acc_fl, acc_fr, acc_rl, acc_rr; //last_steer_angle_ / 180 * M_PI; //
	////end of update ctr

	speed_ = chassis_msg_.speed_mps;
	kappa_ = ref_point_interp_.kappa;
	ref_point_.x_ = ref_point_interp_.x;
	ref_point_.y_ = ref_point_interp_.y;
	ref_point_.theta_ = ref_point_interp_.theta;
	ref_point_.kappa_ = ref_point_interp_.kappa;
	ref_point_.v_ = ref_point_interp_.v;
	ref_point_.s_ = ref_point_interp_.s;
	if (node_yaml_["is_overwrite_speed"].as<int>() == 1)
	{
		ref_point_.a_ = 0.0;
	}
	else
	{
		ref_point_.a_ = ref_point_interp_.a;
	}
	ax_ay_map_["ax"] = localizition_msg_.ax;							  //localizition_msg_.ax;
	ax_ay_map_["ay"] = ref_point_.v_ * ref_point_.v_ * ref_point_.kappa_; //localizition_msg_.ay;
	return true;
}
void NodeLL4d4s::RecordCsv()
{
	Eigen::VectorXd state_esti = mpc_ptr_->kf_lateral_ptr_->state_esti();
	if (node_yaml_["is_log"].as<int>() == 1 && start_log_ == true)
	{
		double speed_limited = eliminate_range<double>(speed_, -1, 0, 1);
		//static io::CSVWriter x0_writer("x0_writer.csv");
		std::vector<std::string> csv_header =
			{
				"s",
				"speed",
				"speed_ref",
				"speed_error", "speed_error_ref", "vy", "vy_ref",
				"yaw_rate", "yaw_rate_ref", "e_y", "e_y_ref",
				"heading_error", "heading_error_ref",
				"delta_f", "delta_f_ref", "delta_r", "delta_r_ref",
				"a_fl", "a_fl_ref", "a_fr", "a_fr_ref",
				"a_rl", "a_rl_ref", "a_rr", "a_rr_ref",
				"a_fl_n", "a_fr_n", "a_rl_n", "a_rr_n",
				"x",
				"y",
				"y_ref",
				"alpha_fl", "alpha_fr", "alpha_rl", "alpha_rr",
				"alpha_fl_n", "alpha_fr_n", "alpha_rl_n", "alpha_rr_n",
				"vx_esti",
				"vy_esti",
				"yaw_rate_esti",
				"ax_dis_esti",
				"ay_dis_esti",
				"yaw_acc_dis_esti",
				"time_to_solve"};
		static bool temp_bool = false;
		if (temp_bool == false)
		{

			log_writer_ptr_->write_row<std::string>(csv_header);
			temp_bool = true;
		}

		std::vector<double> temp_vector =
			{ref_point_closet_.s,
			 speed_, ref_point_closet_.v,
			 x0_[0], mpc_ptr_->x_ref_(0), x0_[1], mpc_ptr_->x_ref_(1),
			 x0_[2], mpc_ptr_->x_ref_(2), x0_[3], mpc_ptr_->x_ref_(3),
			 x0_[4], mpc_ptr_->x_ref_(4),
			 u_fb_[0], mpc_ptr_->u_ref_(0), u_fb_[1], mpc_ptr_->u_ref_(1),
			 u_fb_[2], mpc_ptr_->u_ref_(2), u_fb_[3], mpc_ptr_->u_ref_(3),
			 u_fb_[4], mpc_ptr_->u_ref_(4), u_fb_[5], mpc_ptr_->u_ref_(5),
			 debug_msg_.a_fl, debug_msg_.a_fr, debug_msg_.a_rl, debug_msg_.a_rr,
			 localizition_msg_.x,
			 localizition_msg_.y,
			 ref_lambda_ * ref_point_closet_.y + (1 - ref_lambda_) * ref_point_second_closet_.y,
			 180 / M_PI * (x0_[1] / speed_limited + mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["lf"].as<double>() * x0_[2] / speed_limited - u_fb_(0)),
			 180 / M_PI * (x0_[1] / speed_limited + mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["lf"].as<double>() * x0_[2] / speed_limited - u_fb_(0)),
			 180 / M_PI * (x0_[1] / speed_limited - mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["lr"].as<double>() * x0_[2] / speed_limited - u_fb_(1)),
			 180 / M_PI * (x0_[1] / speed_limited - mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["lr"].as<double>() * x0_[2] / speed_limited - u_fb_(1)),
			 debug_msg_.f_alpha, debug_msg_.f_alpha, debug_msg_.r_alpha, debug_msg_.r_alpha,
			 state_esti(0), state_esti(1), state_esti(2), state_esti(3), state_esti(4), state_esti(5),
			 debug_msg_.time_to_solve};
		log_writer_ptr_->write_row<double>(temp_vector);
	}
}
void NodeLL4d4s::UpdateDebugMsg()
{

	//debug_msg_.tire_f_angle_max_ = tire_f_angle_max_;
	debug_msg_.x = localizition_msg_.x;
	debug_msg_.y = localizition_msg_.y;
	debug_msg_.s = ref_point_interp_.s;
	debug_msg_.a_fl = u_fb_(2) / mpc_ptr_->log_map_["ax_max_fl"];
	debug_msg_.a_fr = u_fb_(3) / mpc_ptr_->log_map_["ax_max_fr"];
	debug_msg_.a_rl = u_fb_(4) / mpc_ptr_->log_map_["ax_max_rl"];
	debug_msg_.a_rr = u_fb_(5) / mpc_ptr_->log_map_["ax_max_rr"];
	kappa_ = ref_point_interp_.kappa;
	Eigen::VectorXd state_esti = mpc_ptr_->kf_lateral_ptr_->state_esti();
	debug_msg_.error_speed = x0_(0);
	debug_msg_.vy_ref = mpc_ptr_->x_ref_(1);
	debug_msg_.yaw_rate_ref = mpc_ptr_->x_ref_(2);
	debug_msg_.lateral_error_ref = mpc_ptr_->x_ref_(3);
	debug_msg_.heading_error_ref = mpc_ptr_->x_ref_(4);

	debug_msg_.vy_esti = state_esti(1);
	debug_msg_.yaw_rate_esti = state_esti(2);
	debug_msg_.acc_x_distur = state_esti(3);
	debug_msg_.acc_y_distur = state_esti(4);
	debug_msg_.acc_yaw_distur = state_esti(5);
	debug_msg_.lateral_speed = x0_(1);
	debug_msg_.yaw_rate = x0_(2);
	debug_msg_.lateral_error = x0_(3);
	debug_msg_.heading_error = x0_(4);
	debug_msg_.kappa = ref_point_interp_.kappa;
	debug_msg_.lambda = ref_lambda_;
	debug_msg_.tire_f_angle_cmd = u_fb_(0) * 180 / 3.14;
	debug_msg_.theta = auto_ros::common_tool::angle_mins_180to180(
		ref_point_closet_.theta);
	debug_msg_.f_angle_ref = mpc_ptr_->u_ref_(0) * 180 / 3.14;
	double temp_speed = 1.0;
	if (std::abs(chassis_msg_.speed_mps) < 1.0)
	{
		if (chassis_msg_.speed_mps < 0)
			temp_speed = -1.0;
		else
			temp_speed = 1.0;
	}
	else
	{
		temp_speed = chassis_msg_.speed_mps;
	}
	double alpha_max = mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["alpha_max"].as<double>() * M_PI / 180.0;
	debug_msg_.f_alpha = (localizition_msg_.vy + mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["lf"].as<double>() * x0_(2)) / temp_speed - u_fb_(0);
	debug_msg_.f_alpha /= alpha_max;
	debug_msg_.r_alpha = (localizition_msg_.vy - mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["lr"].as<double>() * x0_(2)) / temp_speed - u_fb_(1);
	debug_msg_.r_alpha /= alpha_max;
	//ouble temp_f_a = u_fb_(1) / mpc_ptr_->mpc_yaml_["acc_f_x_max"].as < double>();
	//debug_msg_.tire_utilization = std::pow(std::pow(temp_f_n_flapha, 2) + std::pow(temp_f_a, 2), 0.5);
}
bool NodeLL4d4s::LateralCtr()
{
	auto start = std::chrono::high_resolution_clock::now();

	if (!mpc_ptr_->step(x0_, u_fb_, ref_point_, speed_, ax_ay_map_))
	{
		std::cout << "\033[31m NodeLL4d4s::LateralCtr failed, \033[0m" << std::endl;
		return false;
	}
	auto end = std::chrono::high_resolution_clock::now();
	std::chrono::duration<double, std::milli> tm = end - start;
	debug_msg_.time_to_solve = tm.count();

	std::cout << "##########################" << tm.count() << std::endl;									// 毫秒
	control_cmd_.tire_angle_target_f = mpc_ptr_->vehicle_mpc_ptr_->get_contorl_one_shot(0)(0) * 180 / 3.14; //low_pass_f_.filter(mpc_ptr_->vehicle_mpc_ptr_->get_contorl_one_shot(0)(0) * 180 / 3.14); //;vehicle_mpc_ptr_->get_optimal_ctr()(0, 0) / rate_ * 180 / 3.14;
	control_cmd_.tire_angle_target_r = mpc_ptr_->vehicle_mpc_ptr_->get_contorl_one_shot(0)(1) * 180 / 3.14;
	//计算驱动力距和制动压力
	std::vector<double> tires_torque;
	std::vector<double> tires_brake_pre;
	for (int tire_index = 0; tire_index < 4; tire_index++)
	{
		double acc_cmd = mpc_ptr_->vehicle_mpc_ptr_->get_contorl_one_shot(0)(2 + tire_index);
		double torque;
		double brake_pre;
		if (acc_cmd >= 0)
		{
			torque = acc_cmd * mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["mass"].as<double>() *
					 mpc_ptr_->mpc_yaml_["tire_raduis"].as<double>();
			brake_pre = 0; //Mpa
		}
		else
		{
			torque = 0;
			brake_pre = std::abs(acc_cmd) * mpc_ptr_->vehicle_dyn_ptr_->dyn_yaml_["mass"].as<double>() *
						mpc_ptr_->mpc_yaml_["tire_raduis"].as<double>() /
						mpc_ptr_->mpc_yaml_["brake_nm_per_Mpa"].as<std::vector<double>>()[tire_index];
		}
		tires_torque.push_back(torque);
		tires_brake_pre.push_back(brake_pre);
	}
	control_cmd_.torque_fl = tires_torque[0];
	control_cmd_.torque_fr = tires_torque[1];
	control_cmd_.torque_rl = tires_torque[2];
	control_cmd_.torque_rr = tires_torque[3];

	control_cmd_.brake_pre_fl = tires_brake_pre[0];
	control_cmd_.brake_pre_fr = tires_brake_pre[1];
	control_cmd_.brake_pre_rl = tires_brake_pre[2];
	control_cmd_.brake_pre_rr = tires_brake_pre[3];

	return true;
}
bool NodeLL4d4s::LongtitudeCtr()
{
	return true;
}
void NodeLL4d4s::LoopProc()
{

	UpdateDebugMsg();

	debug_pub_.publish(debug_msg_);
	debug_msg_.is_solved = 0.0;

	/*if (chassis_msg_.driving_mode != 1)
	{
		std::cout << "\033[31m NodeLL4d4s::LoopProc  failed,chassis_msg_.driving_mode != 1(not auto control) \033[0m" << std::endl;
		mpc_ptr_->reset_kf();
		return;
	}*/
	NoFood();
	if (!CheckDog())
	{
		std::cout << "\033[31m NodeLL4d4s::LoopProc-CheckDog  failed, \033[0m" << std::endl;
		HardBrake();
		mpc_ptr_->reset_kf();
		control_cmd_pub_.publish(control_cmd_);
		return;
	}

	if (!SearchRefPoint(planning_msg_))
	{
		std::cout << "\033[31m NodeLL4d4s::LoopProc-SearchRefPoint  failed, \033[0m" << std::endl;
		HardBrake();
		control_cmd_pub_.publish(control_cmd_);
		mpc_ptr_->reset_kf();
		return;
	}

	if (!UpdateInput())
	{
		std::cout << "\033[31m NodeLL4d4s::LoopProc-UpdateInput  failed, \033[0m" << std::endl;
		HardBrake();
		control_cmd_pub_.publish(control_cmd_);
		mpc_ptr_->reset_kf();
		return;
	}

	/*if (auto_msg_dog_.isDied() || auto_ctr_msg_.ctr_mode <= 1)
	{
		start_log_ = false;
		std::cout << "\033[31m NodeLL4d4s::auto_msg_dog_ timeout  failed, \033[0m" << std::endl;
		HardBrake(0.3);
		control_cmd_pub_.publish(control_cmd_);
		mpc_ptr_->reset_kf();
		return;
	}
	else
	{*/

	RecordCsv();
	start_log_ = true;
	if (!LateralCtr())
	{
		std::cout << "\033[31m NodeLL4d4s::LoopProc-LateralCtr  failed, \033[0m" << std::endl;
		mpc_ptr_->reset_kf();
		HardBrake();
		//control_cmd_pub_.publish(control_cmd_);
		return;
	}
	LongtitudeCtr();
	control_cmd_pub_.publish(control_cmd_);
	debug_msg_.is_solved = 1.0;
}
} // namespace control
} // namespace auto_ros